#include <ctime>
#include "KinectWrapper.h"
#include "DataWriter.h"
#include "../calibration/DataReader.h"

using namespace std;
using namespace cv;

const int WIDTH = 640;
const int HEIGHT = 480;
CvMat *depthFrame = NULL;
CvMat *grayFrame = NULL;
CvMat *colorFrame = NULL;
CvMat *dcMappedFrame = NULL;

/*

HANDLE depthReadyEvent;
HANDLE grayReadyEvent;
HANDLE colorReadyEvent;

HANDLE depthHandledEvent;
HANDLE grayHandledEvent;
HANDLE colorHandledEvent;

HANDLE depthQuitEvent;
HANDLE grayQuitEvent;
HANDLE colorQuitEvent;

HANDLE depthColorHandledEvent;
bool quit = false;
bool saveDCData = false;
bool saveGData = false;

DWORD WINAPI keepDepthData(void *pParam) {
	KinectDepthWrapper depthReader;
	FILE *dataFile = NULL;
	FILE *metaDataFile = NULL;
	int cnt = 0;
	SYSTEMTIME st;

	dataFile = fopen("depth_data.dat", "wb");
	metaDataFile = fopen("depth_meta.txt", "w");

	fprintf(metaDataFile, "#first line include width height. next line is ID CLOCK TIME and so on. the last line is total frame number.\n");
	fprintf(metaDataFile, "%d %d\n", WIDTH, HEIGHT);
	
	depthReader.init(WIDTH, HEIGHT);

	SignalObjectAndWait(depthReadyEvent, depthHandledEvent, INFINITE, FALSE);
	while (!quit) {
		if (depthReader.retrieve(depthFrame)) {
			GetSystemTime(&st);
			int clk = clock();
			fwrite(depthFrame->data.s, sizeof(short), WIDTH*HEIGHT, dataFile);
			fprintf(metaDataFile, "%5d %9d   %02d/%02d %02d:%02d:%02d.%03d\n", cnt++, clk, 
				st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond, st.wMilliseconds);

			SignalObjectAndWait(depthReadyEvent, depthHandledEvent, 1000, FALSE);
		}
		else {
			cout << "retrieve depth failed." << endl;
		}
	}
	fprintf(metaDataFile, "#total frames = %d.", cnt);

	fclose(dataFile);
	fclose(metaDataFile);
	SetEvent(depthQuitEvent);
	return 0;
}

CvCapture* capture;

DWORD WINAPI keepGrayData(void *pParam) {
	FILE *dataFile = NULL;
	FILE *metaDataFile = NULL;
	int cnt = 0;
	SYSTEMTIME st;

	dataFile = fopen("gray_data.dat", "wb");
	metaDataFile = fopen("gray_meta.txt", "w");

	fprintf(metaDataFile, "#first line include width height. next line is ID CLOCK TIME and so on. the last line is total frame number.\n");
	fprintf(metaDataFile, "%d %d\n", WIDTH, HEIGHT);

	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, WIDTH);
	cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, HEIGHT);

	CvMat *frameMat = cvCreateMatHeader(HEIGHT, WIDTH, CV_8UC3);
	grayFrame = cvCreateMat(HEIGHT, WIDTH, CV_8UC1);

	SignalObjectAndWait(grayReadyEvent, grayHandledEvent, INFINITE, FALSE);
	while (!quit) {
		IplImage* frame = cvQueryFrame(capture);

		GetSystemTime(&st);
		int clk = clock();

		cvGetMat(frame, frameMat);
		cvCvtColor(frameMat, grayFrame, CV_BGR2GRAY);
		cvFlip(grayFrame, grayFrame, 1);

		SignalObjectAndWait(grayReadyEvent, grayHandledEvent, 2000, FALSE);

		if (1) {
			saveGData = false;
			fwrite(grayFrame->data.ptr, sizeof(char), WIDTH*HEIGHT, dataFile);
			fprintf(metaDataFile, "%5d %9d   %02d/%02d %02d:%02d:%02d.%03d\n", cnt++, clk, 
				st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond, st.wMilliseconds);
		}

	}
	fprintf(metaDataFile, "#total frames = %d.", cnt);

	fclose(dataFile);
	fclose(metaDataFile);

	cvReleaseCapture(&capture);

	SetEvent(grayQuitEvent);
	return 0;
}

DWORD WINAPI keepDepthColorData(void *pParam) {
	KinectDepthColorWrapper depthColorReader;
	FILE *dataFile = NULL;
	FILE *metaDataFile = NULL;
	FILE *colorDataFile = NULL;
	FILE *colorMetaDataFile = NULL;
	int cnt = 0;
	SYSTEMTIME st;

	CvMat *map1 = cvCreateMat(HEIGHT, WIDTH, CV_32S);
	CvMat *map2 = cvCloneMat(map1);


	dataFile = fopen("depth_data.dat", "wb");
	metaDataFile = fopen("depth_meta.txt", "w");
	colorDataFile = fopen("color_data.dat", "wb");
	colorMetaDataFile = fopen("color_meta.txt", "w");

	fprintf(metaDataFile, "#first line include width height. next line is ID CLOCK TIME and so on. the last line is total frame number.\n");
	fprintf(metaDataFile, "%d %d\n", WIDTH, HEIGHT);
	fprintf(colorMetaDataFile, "#first line include width height. next line is ID CLOCK TIME and so on. the last line is total frame number.\n");
	fprintf(colorMetaDataFile, "%d %d\n", WIDTH, HEIGHT);

	depthColorReader.init();

	SetEvent(colorReadyEvent);
	SignalObjectAndWait(depthReadyEvent, depthColorHandledEvent, INFINITE, FALSE);
	
	while (!quit) {
		if (depthColorReader.retrieve(depthFrame, colorFrame, dcMappedFrame)) {

			GetSystemTime(&st);
			int clk = clock();

			SetEvent(colorReadyEvent);
			SignalObjectAndWait(depthReadyEvent, depthColorHandledEvent, 2000, FALSE);

			if (1) {
				saveDCData = false;
				fwrite(dcMappedFrame->data.s, sizeof(short), WIDTH*HEIGHT, dataFile);
				fprintf(metaDataFile, "%5d %9d  %lld  %02d/%02d %02d:%02d:%02d.%03d\n", cnt, clk, depthColorReader.depthT,
					st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond, st.wMilliseconds);
// 				fwrite(colorFrame->data.ptr, sizeof(char), WIDTH*HEIGHT*3, colorDataFile);
// 				fprintf(colorMetaDataFile, "%5d %9d  %lld  %02d/%02d %02d:%02d:%02d.%03d\n", cnt++, clk, depthColorReader.colorT,
// 					st.wMonth, st.wDay, st.wHour, st.wMinute, st.wSecond, st.wMilliseconds);
			}

		}
		else {
			cout << "retrieve depth failed." << endl;
		}
	}
	fprintf(metaDataFile, "#total frames = %d.", cnt);
	fprintf(colorMetaDataFile, "#total frames = %d.", cnt);

	fclose(dataFile);
	fclose(metaDataFile);
	SetEvent(depthQuitEvent);
	fclose(colorDataFile);
	fclose(colorMetaDataFile);
	SetEvent(colorQuitEvent);
	return 0;
}

void KeepThreeTypeData(void)
{
	cvNamedWindow("aligned");

	cvNamedWindow("depth");
	depthReadyEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	// 	depthHandledEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	depthQuitEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	cvNamedWindow("color");
	colorReadyEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	depthColorHandledEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	colorQuitEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	CreateThread(NULL, 0, keepDepthColorData, NULL, 0, NULL);

	cvNamedWindow("gray");
	grayReadyEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	grayHandledEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	grayQuitEvent = CreateEvent(NULL, FALSE, FALSE, NULL);
	CreateThread(NULL, 0, keepGrayData, NULL, 0, NULL);


	HANDLE handles[] = {depthReadyEvent, grayReadyEvent, colorReadyEvent};
	int handleNums = sizeof(handles) / sizeof(HANDLE);

	WaitForMultipleObjects(handleNums, handles, TRUE, INFINITE);
	SetEvent(depthColorHandledEvent);
	SetEvent(grayHandledEvent);

	while (!quit) {
		WaitForMultipleObjects(handleNums, handles, TRUE, INFINITE);

		cvShowImage("dcMapped", dcMappedFrame);
		cvShowImage("gray", grayFrame);
		cvShowImage("color", colorFrame);
		SetEvent(depthColorHandledEvent);
		SetEvent(grayHandledEvent);
		char c = cvWaitKey(30);
		if (c == 27) quit = true;
	}

	handles[0] = depthQuitEvent;
	handles[1] = grayQuitEvent;
	handles[2] = colorQuitEvent;
	SetEvent(depthColorHandledEvent);
	SetEvent(grayHandledEvent);
	WaitForMultipleObjects(handleNums, handles, TRUE, 5000);
}

*/

void KeepKinectData(void)
{
	KinectDepthColorWrapper kinectDeviceReader;
	kinectDeviceReader.init();

	DataWriter depthWriter("depth_data.dat", "depth_meta.txt", "depth");
	depthWriter.Init(WIDTH, HEIGHT, 1);

	DataWriter colorWriter("color_data.dat", "color_meta.txt", "color");
	colorWriter.Init(WIDTH, HEIGHT, 3);

	while (kinectDeviceReader.retrieve(depthFrame, colorFrame, dcMappedFrame))
	{
		depthWriter.Write(dcMappedFrame->data.s, kinectDeviceReader.depthT);
		colorWriter.Write(colorFrame->data.ptr, kinectDeviceReader.colorT);
		cvShowImage("depth", dcMappedFrame);
		cvShowImage("color", colorFrame);
		unsigned char ch = cvWaitKey(30);
		if (ch == 27) break;
	}

	cvDestroyAllWindows();

	depthWriter.Release();

	DepthReader depthReader;
	while (true)
	{
		bool status = depthReader.getNextFrame(depthFrame->data.s);
		cout << "status: " << status << endl;
		if (!status) break;
		cvShowImage("debug", depthFrame);
		cvWaitKey(30);
	}

	cvDestroyAllWindows();
}

int main(void)
{
	//the camera must initialize here, or it can't setup resolution!!!
// 	capture = cvCaptureFromCAM(CV_CAP_DSHOW+1); // capture from video device #0
//	KeepThreeTypeData();
	KeepKinectData();
	return 0;
}
